/**************************************************************
 * Ben Stephenson       07025656
 * Mark Renney          07010390
 * Laurie Potts         07009314
 * Michael Peeke-Vout   07014312
 *
 * CM0034 Industrial Case Project
 * Robot Movements
 * robot_movements.c
 * 28/02/2011
 * Updated - 16/05/11 
 **************************************************************/

#include <robot_movements.h>

//Function to move the robot arm to set positions
void MoveRobot (robotJoint_t joint_value, uint32_t move_to) {
  while(true) {
    //Get current state of the joint
    uint32_t joint_current_position = robotJointGetState(joint_value);
    //If the state of the joint is less than where the new joint position
    if (joint_current_position < move_to) {
      robotJointSetState((robotJoint_t)joint_value, (robotJointStep_t)ROBOT_JOINT_POS_INC);
      joint_current_position = robotJointGetState(joint_value);           
    }
    //If the state of the joint is more than where the new joint position
    else if (joint_current_position > move_to) {
      robotJointSetState((robotJoint_t)joint_value, (robotJointStep_t)ROBOT_JOINT_POS_DEC);
      joint_current_position = robotJointGetState(joint_value);
    }
    if(move_to  == joint_current_position) {
      break;
    }
     OSTimeDly(16);
  }
}

//Move Robot To Default Positions
void Default_Positions (void) {
      //Move Waist To Middle
      MoveRobot(ROBOT_WAIST, WAIST_DEFAULT);
      //Move Elbow To Middle
      MoveRobot(ROBOT_ELBOW, ELBOW_DEFAULT);
      //Move Wrist To Middle
      MoveRobot(ROBOT_WRIST, WRIST_DEFAULT);
      //Move Hand To Middle
      MoveRobot(ROBOT_HAND, JOINT_MAXPOS);
}

//Move Robot To Move Block To Conveyor
void Robot_Collect_Block_Part1 (void) {
      //Move Robot To Default
      Default_Positions();
      //Move Hand To Open
      MoveRobot(ROBOT_HAND, JOINT_MINPOS);
      //Move Waist To Input
      MoveRobot(ROBOT_WAIST, JOINT_MAXPOS);
      //Move Elbow To Input
      MoveRobot(ROBOT_ELBOW, JOINT_MAXPOS);
      //Move Wrist To Input
      MoveRobot(ROBOT_WRIST, JOINT_MAXPOS);
      //Squeeze Hand Together
      MoveRobot(ROBOT_HAND, JOINT_MAXPOS);
      //Raise Elbow To Default
      MoveRobot(ROBOT_ELBOW, LIFT_HEIGHT_INPUT);
}
      
void Robot_Collect_Block_Part2 (void) {     
      //Move Wrist To Default
      MoveRobot(ROBOT_WAIST, INPUT_TO_CONVEYOR);
      //Move Elbow To Output
      MoveRobot(ROBOT_ELBOW, JOINT_MAXPOS);
      //Move Wrist To Output
      MoveRobot(ROBOT_HAND, JOINT_MINPOS);
      //Raise Elbow To Default
      MoveRobot(ROBOT_ELBOW, LIFT_HEIGHT_INPUT);
      //Squeeze Hand Together
      MoveRobot(ROBOT_HAND, JOINT_MAXPOS);
      //Move Wrist To Default
      Default_Positions();
}

//Move Robot To Remove Block
void Robot_Remove_Block_Part1 (void) {
      //Move Robot To Default
      Default_Positions();
      //Move Hand To Open
      MoveRobot(ROBOT_HAND, JOINT_MINPOS);
      //Move Waist To Conveyor
      MoveRobot(ROBOT_WAIST, OUTPUT_TO_CONVEYOR);
      //Move Elbow To Conveyor
      MoveRobot(ROBOT_ELBOW, ELBOW_DROP_INPUT);
      //Move Wrist To Conveyor
      MoveRobot(ROBOT_WRIST, WRIST_DROP_INPUT);
      //Squeeze Hand Together
      MoveRobot(ROBOT_HAND, HAND_SAFE);
      //Move Wrist To Default
      MoveRobot(ROBOT_ELBOW, DEFAULT_MIDDLE);
      //Open Hand To Release
      MoveRobot(ROBOT_WRIST, JOINT_MAXPOS);
}    
      
void Robot_Remove_Block_Part2 (void) {      
      //Move Waist To Conveyor
      MoveRobot(ROBOT_WAIST, CONVEYOR_TO_OUTPUT);
      //Move Elbow To Output
      MoveRobot(ROBOT_ELBOW, ELBOW_DROP_SEVERE_OUTPUT);
      //Open Hand To Release
      MoveRobot(ROBOT_HAND, JOINT_MINPOS);
      //Raise Elbow To Default
      MoveRobot(ROBOT_ELBOW, ELBOW_DEFAULT_OUTPUT);
      //Move Wrist To Default
      MoveRobot(ROBOT_WRIST, WRIST_DEFAULT_OUTPUT);
      //Return Robot To Default
      Default_Positions();
}

//Set the robot state to idle
static uint8_t robotState = IDLE;
static bool pad[2];

//Set the system pads to empty
void initRobotVars(){
  robotState = IDLE;
  pad[IN_PAD] = false;
  pad[CON_PAD] = false;
}

//Return the state of the robot
uint8_t getRobotState(void){
  return robotState;
}
//Set the state of the robot
void setRobotState(uint8_t tState){
  robotState = tState;
}

//Get the state of the pad
bool getPadState(uint8_t padNum){
  return pad[padNum]; 
}

//Set the state of the pad
void setPadState(uint8_t padNum, bool padState){
  pad[padNum] = padState;
}